// controller.cpp
#include "controller.h"

#include <algorithm>
#include <atomic>
#include <chrono>
#include <cmath>
#include <cstdint>
#include <cstring>
#include <fstream>
#include <iostream>
#include <thread>
#include <cstdlib>   // std::getenv, std::stod
#include <filesystem>
#include <sstream>
#include <iomanip>
#include <regex>
#include <deque>     // <-- 新增：用于稳定性窗口

#include <nlohmann/json.hpp>

#include "kz_io.h"
#include "struct_resource.h"

#if defined(_WIN32)
  #include <winsock2.h>
  #include <ws2tcpip.h>
  #pragma comment(lib, "Ws2_32.lib")
  using socklen_t = int;
#else
  #include <arpa/inet.h>
  #include <netinet/in.h>
  #include <sys/socket.h>
  #include <sys/types.h>
  #include <unistd.h>
#endif

using nlohmann::json;

namespace tsync {

// ---- UDP 简易封装 ----
static int udp_bind_any(const char* ip, uint16_t port) {
  int fd = (int)socket(AF_INET, SOCK_DGRAM, 0);
  if (fd < 0) { perror("socket"); return -1; }
  sockaddr_in addr{}; addr.sin_family = AF_INET; addr.sin_port = htons(port);
  if (inet_pton(AF_INET, ip, &addr.sin_addr) != 1) { perror("inet_pton"); return -1; }
  int on = 1; setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, (const char*)&on, sizeof(on));
  if (bind(fd, (sockaddr*)&addr, sizeof(addr)) < 0) { perror("bind"); return -1; }
  return fd;
}
static sockaddr_in make_dst(const std::string& ip, uint16_t port) {
  sockaddr_in dst{}; dst.sin_family = AF_INET; dst.sin_port = htons(port);
  if (inet_pton(AF_INET, ip.c_str(), &dst.sin_addr) != 1) { perror("inet_pton"); }
  return dst;
}

// ---- 简单离散滤波器：Direct Form II (Transposed) 用于 K(z) ----
struct DF2T {
  std::vector<double> a, b;   // a0..an, b0..bm 其中 a0=1
  std::vector<double> s;      // 状态（长度 = max(nA, nB)）

  DF2T() = default;
  explicit DF2T(const TF& H) { init(H); }

  void init(const TF& H) {
    if (H.den.empty() || std::abs(H.den[0]) < 1e-15) {
      throw std::runtime_error("DF2T: den[0] must be nonzero");
    }
    a = H.den; b = H.num;
    const double a0 = a[0];
    if (std::abs(a0 - 1.0) > 1e-12) {
      for (double& v : a) v /= a0;
      for (double& v : b) v /= a0;
    }
    const size_t nA = a.size() ? a.size() - 1 : 0;
    const size_t nB = b.size() ? b.size() - 1 : 0;
    s.assign(std::max(nA, nB), 0.0);
  }

  // 单步：输入 e（ns） -> 输出 μ（ns）
  double step(double e) {
    const double b0 = b.empty() ? 0.0 : b[0];
    double y = b0 * e + (s.empty() ? 0.0 : s[0]);
    const size_t n = s.size();
    for (size_t i = 0; i < n; ++i) {
      const double bi1 = (i+1 < b.size()) ? b[i+1] : 0.0;
      const double ai1 = (i+1 < a.size()) ? a[i+1] : 0.0;
      const double nx  = (i+1 < n) ? s[i+1] : 0.0;
      s[i] = nx + bi1 * e - ai1 * y;
    }
    return y;
  }
  void reset() { std::fill(s.begin(), s.end(), 0.0); }
};

// ---- 读取邻居 JSON（旧格式）----
static bool load_neighbors_json(const std::string& path,
                                std::vector<Neighbor>& out,
                                std::string* err_msg = nullptr)
{
  std::ifstream ifs(path);
  if (!ifs) {
    if (err_msg) *err_msg = "cannot open file: " + path;
    return false;
  }
  json j;
  try { ifs >> j; }
  catch (const std::exception& e) {
    if (err_msg) *err_msg = std::string("json parse error: ") + e.what();
    return false;
  }
  if (!j.contains("neighbors") || !j["neighbors"].is_array()) {
    if (err_msg) *err_msg = R"(missing array field "neighbors")";
    return false;
  }
  std::vector<Neighbor> tmp;
  for (const auto& it : j["neighbors"]) {
    if (!it.contains("ip") || !it.contains("port")) {
      if (err_msg) *err_msg = R"(neighbor item missing "ip" or "port")";
      return false;
    }
    if (!it["ip"].is_string() || !(it["port"].is_number_unsigned() || it["port"].is_number_integer())) {
      if (err_msg) *err_msg = R"("ip" must be string and "port" must be integer)";
      return false;
    }
    std::string ip = it["ip"].get<std::string>();
    uint32_t port_u32 = it["port"].get<uint32_t>();
    if (port_u32 == 0 || port_u32 > 65535) {
      if (err_msg) *err_msg = "port out of range: " + std::to_string(port_u32);
      return false;
    }
    tmp.push_back(Neighbor{ip, static_cast<uint16_t>(port_u32)});
  }
  out = std::move(tmp);
  return true;
}

// ---- 读取分组（方案B）----
static bool load_neighbor_groups_json(const std::string& path,
                                      std::vector<NeighborGroup>& out,
                                      std::string* err_msg = nullptr)
{
  std::ifstream ifs(path);
  if (!ifs) { if (err_msg) *err_msg = "cannot open file: " + path; return false; }

  json j;
  try { ifs >> j; }
  catch (const std::exception& e) {
    if (err_msg) *err_msg = std::string("json parse error: ")+e.what();
    return false;
  }

  if (!j.contains("groups") || !j["groups"].is_array()) {
    if (err_msg) *err_msg = R"(missing or invalid "groups")";
    return false;
  }

  std::vector<NeighborGroup> groups;
  for (const auto& g : j["groups"]) {
    if (!g.is_object() || !g.contains("peers") || !g["peers"].is_array()) {
      if (err_msg) *err_msg = R"(each group must be {name?, peers:[]})";
      return false;
    }
    NeighborGroup G;
    G.name = g.value("name", std::string{});

    for (const auto& it : g["peers"]) {
      if (!it.contains("ip") || !it.contains("port")) {
        if (err_msg) *err_msg = R"(peer needs "ip" and "port")";
        return false;
      }
      if (!it["ip"].is_string() ||
          !(it["port"].is_number_unsigned() || it["port"].is_number_integer())) {
        if (err_msg) *err_msg = R"("ip" must be string and "port" must be integer)";
        return false;
      }
      uint32_t p = it["port"].get<uint32_t>();
      if (p == 0 || p > 65535) { if (err_msg) *err_msg = "port out of range"; return false; }

      G.peers.push_back(Neighbor{ it["ip"].get<std::string>(),
                                  static_cast<uint16_t>(p) });
    }
    groups.push_back(std::move(G));
  }

  if (groups.empty()) { if (err_msg) *err_msg = "groups empty"; return false; }
  out = std::move(groups);
  return true;
}

// ---- 运行标志（两个线程共享）----
static std::atomic<bool> g_running{true};

// --- 日志目录与 SVG 写入工具 ---
static void ensure_log_dir(const std::string& dir) {
  try { std::filesystem::create_directories(dir); } catch (...) {}
}

// static void write_eavg_svg(const std::vector<double>& e_hist,
//                            const std::string& svg_path,
//                            const std::string& title)
// {
//   const int W = 1200, H = 420, M = 48;
//   if (e_hist.empty()) return;
//
//   double ymin = *std::min_element(e_hist.begin(), e_hist.end());
//   double ymax = *std::max_element(e_hist.begin(), e_hist.end());
//   if (ymin == ymax) { ymin -= 1.0; ymax += 1.0; }
//   const double pad = 0.10*(ymax - ymin);
//   ymin -= pad; ymax += pad;
//
//   auto xmap = [&](int i)->double{
//     if (e_hist.size() == 1) return (double)M;
//     return M + (double)i * (double)(W - 2*M) / (double)(e_hist.size() - 1);
//   };
//   auto ymap = [&](double v)->double{
//     double t = (v - ymin) / (ymax - ymin);
//     return (double)H - M - t * (double)(H - 2*M);
//   };
//
//   std::ostringstream pts;
//   pts.setf(std::ios::fixed); pts<<std::setprecision(1);
//   for (size_t i=0;i<e_hist.size();++i) {
//     pts << xmap((int)i) << "," << ymap(e_hist[i]);
//     if (i+1<e_hist.size()) pts << " ";
//   }
//
//   std::ostringstream ss;
//   ss.setf(std::ios::fixed);
//   ss << "<svg xmlns='http://www.w3.org/2000/svg' width='"<<W<<"' height='"<<H<<"'>\n"
//      << "<rect x='0' y='0' width='"<<W<<"' height='"<<H<<"' fill='white'/>\n"
//      << "<text x='"<<(W/2)<<"' y='28' text-anchor='middle' font-family='monospace' font-size='16'>"
//      << title << "</text>\n"
//      << "<rect x='"<<M<<"' y='"<<M<<"' width='"<<(W-2*M)<<"' height='"<<(H-2*M)
//      <<"' fill='none' stroke='black' stroke-width='1'/>\n"
//      << "<g stroke='#ccc' stroke-width='0.6'>\n";
//   for (int g=0; g<=5; ++g) {
//     double yy = M + (double)g * (H - 2*M) / 5.0;
//     ss << "<line x1='"<<M<<"' y1='"<<yy<<"' x2='"<<(W-M)<<"' y2='"<<yy<<"'/>\n";
//   }
//   ss << "</g>\n";
//
//   ss<<std::setprecision(0)<<"<g fill='#333' font-family='monospace' font-size='12'>\n";
//   for (int g=0; g<=5; ++g) {
//     double val = ymax - (double)g * (ymax - ymin) / 5.0;
//     double yy  = M   + (double)g * (H - 2*M) / 5.0;
//     ss << "<text x='"<<(M-6)<<"' y='"<<(yy+4)<<"' text-anchor='end'>"<< val <<" ns</text>\n";
//   }
//   ss << "</g>\n";
//
//   ss << "<polyline fill='none' stroke='#1976d2' stroke-width='1.8' points='"<<pts.str()<<"'/>\n";
//
//   const size_t N = e_hist.size();
//   double x_last = xmap((int)N-1), y_last = ymap(e_hist.back());
//   ss << "<circle cx='"<<x_last<<"' cy='"<<y_last<<"' r='3' fill='#d32f2f'/>\n"
//      << "<text x='"<<(x_last+8)<<"' y='"<<(y_last-6)
//      <<"' font-family='monospace' font-size='12' fill='#d32f2f'>k="<<(N-1)
//      <<", e_avg="<<e_hist.back()<<" ns</text>\n</svg>\n";
//
//   std::ofstream ofs(svg_path, std::ios::binary);
//   if (ofs) ofs << ss.str();
// }

// ---- CSV（增强版）：写入表头并追加指标 ----
static void append_metrics_csv_ex(const std::string& csv_path,
                                  uint32_t k,
                                  double mean_abs_e,
                                  double mean_e,
                                  double max_e,
                                  double min_e,
                                  int64_t t_logic_before_ns,  // 写入时钟更新前的 L(t)
                                  int64_t t_raw_ns,
                                  int64_t offset_before_ns,   // 更新前的偏移
                                  double  slope_ppm,          // 更新后的斜率（ppm）
                                  int64_t t_logic_after_ns,   // 更新后的 L(t)
                                  int64_t offset_after_ns)    // 更新后的偏移
{
  bool need_header = !std::filesystem::exists(csv_path);
  std::ofstream ofs(csv_path, std::ios::app);
  if (!ofs) return;

  if (need_header) {
    ofs << "k,abs_mean_e_ns,mean_e_ns,max_e_ns,min_e_ns,"
           "t_logic_before_ns,t_raw_ns,offset_before_ns,"
           "slope_ppm,t_logic_after_ns,offset_after_ns\n";
  }
  ofs.setf(std::ios::fixed);
  ofs << k << ","
      << std::setprecision(9) << mean_abs_e << ","
      << std::setprecision(9) << mean_e     << ","
      << std::setprecision(9) << max_e      << ","
      << std::setprecision(9) << min_e      << ","
      << t_logic_before_ns     << ","
      << t_raw_ns              << ","
      << offset_before_ns      << ","
      << std::setprecision(6)  << slope_ppm  << ","
      << t_logic_after_ns      << ","
      << offset_after_ns       << "\n";
}

// ---- 常驻被动回包线程（固定端口，随到随回）----
static void responder_loop(const char* bind_ip, uint16_t listen_port, LogicalClock& lc) {

#if defined(_WIN32)
  WSADATA wsa; WSAStartup(MAKEWORD(2,2), &wsa);
#endif
  int fd = udp_bind_any(bind_ip, listen_port);
  if (fd < 0) {
    std::cerr << "[responder] bind failed on " << bind_ip << ":" << listen_port << "\n";
    return;
  }
  std::cout << "[responder] listening on " << bind_ip << ":" << listen_port << "\n";

  while (g_running.load(std::memory_order_relaxed)) {
    SyncNtpMsg req_net{}; sockaddr_in src{}; socklen_t sl=sizeof(src);
    ssize_t n = recvfrom(fd, (char*)&req_net, sizeof(req_net), 0, (sockaddr*)&src, &sl);
    if (n < (ssize_t)sizeof(SyncNtpMsg)) continue;

    SyncNtpMsg req = req_net; req.ntoh();
    if (req.mode != 1) continue;  // 只响应 REQ

    // 构造 RESP（即时）
    SyncNtpMsg resp = SyncNtpMsg::make_resp_from_req(req);
    resp.t2_ns = (uint64_t)lc.now_ns();
    resp.t3_ns = (uint64_t)lc.now_ns();

    SyncNtpMsg resp_net = resp; resp_net.hton();
    if (sendto(fd, (const char*)&resp_net, sizeof(resp_net), 0, (sockaddr*)&src, sl) < 0) {
      perror("[responder] sendto");
    }
  }

#if defined(_WIN32)
  closesocket(fd);
#else
  close(fd);
#endif
}

// ---- 工具：把组名变为安全文件名片段 ----
static std::string sanitize_group_name(const std::string& name) {
  std::string out; out.reserve(name.size());
  for (char c : name) {
    if ((c>='A' && c<='Z') || (c>='a' && c<='z') || (c>='0' && c<='9') || c=='_' || c=='-')
      out.push_back(c);
  }
  return out;
}

// 返回某组的日志目录，例如: ../data/log/grp_A/ ；若组名为空，用 g0/
static std::string group_log_dir(size_t gid, const std::string& gname) {
  std::string s = sanitize_group_name(gname);
  if (s.empty()) {
    std::ostringstream p; p << "../data/log/g" << gid << "/";
    return p.str();
  } else {
    std::ostringstream p; p << "../data/log/" << s << "/";
    return p.str();
  }
}

// ---- 周期性主动对齐线程（每 τ 发 REQ，只收 RESP，做估计+控制）----
// 每个组独立扇出/扇入/控制，且每组使用自己的 K(z) 文件：../data/kz<groupname>.json
static void requester_loop(std::vector<Neighbor>& neighbors,
                           uint32_t tau_ns,
                           const char* bind_ip,
                           int recv_window_ms,
                           double k_limit,
                           LogicalClock& lc)
{
  (void)neighbors; // 兼容旧接口形参

  struct PeerDetail {
    std::string ip;
    uint16_t port{};
    uint64_t t1_ns{}, t2_ns{}, t3_ns{}, t4_ns{};
    int64_t offset_ns{}, delay_ns{};
    double  error_ns{};
  };

#if defined(_WIN32)
  WSADATA wsa; WSAStartup(MAKEWORD(2,2), &wsa);
#endif

  LogicalClock lc_raw(/*init_offset_ns=*/0);  // 影子时钟：不做 add_offset_step

  // 绑定随机端口：用于发送 REQ / 接收 RESP（与 Responder 独立）
  int fd = udp_bind_any(bind_ip, 0);
  if (fd < 0) throw std::runtime_error("requester: udp_bind_any failed");

  // 接收超时：短超时便于窗口轮询
#if defined(_WIN32)
  DWORD tv = 5;  setsockopt(fd, SOL_SOCKET, SO_RCVTIMEO, (const char*)&tv, sizeof(tv));
#else
  timeval tv{}; tv.tv_sec = 0; tv.tv_usec = 5 * 1000; // 5ms
  setsockopt(fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv));
#endif

  // 加载邻居分组
  std::string err;
  const std::string nb_path = "../data/neighbors.json";

  std::vector<NeighborGroup> groups;
  if (!load_neighbor_groups_json(nb_path, groups, &err)) {
    std::vector<Neighbor> flat;
    if (!load_neighbors_json(nb_path, flat, &err)) {
      throw std::runtime_error("requester: load neighbors failed: " + err + " (" + nb_path + ")");
    }
    NeighborGroup g; g.name = ""; g.peers = std::move(flat);
    groups.push_back(std::move(g));
  }

  // 预构建每组的 sockaddr 列表（与名称并行）
  std::vector<std::vector<sockaddr_in>> dst_groups;
  std::vector<std::string> group_names;
  dst_groups.reserve(groups.size());
  group_names.reserve(groups.size());
  for (auto& g : groups) {
    std::vector<sockaddr_in> vec; vec.reserve(g.peers.size());
    for (auto& nb : g.peers) vec.emplace_back(make_dst(nb.ip, nb.port));
    dst_groups.push_back(std::move(vec));
    group_names.push_back(std::move(g.name));
  }

  // 为每个组加载控制器 K(z)
  std::vector<DF2T> filts; filts.reserve(dst_groups.size());
  std::vector<std::string> kz_used_path(dst_groups.size());
  for (size_t gid=0; gid<dst_groups.size(); ++gid) {
    TF kz{}; double gamma_in_file = NAN;

    std::string sanitized = sanitize_group_name(group_names[gid]);
    std::string prefer_path = sanitized.empty()
                              ? "../data/kz.json"
                              : ("../data/kz" + sanitized + ".json");

    bool ok = load_kz_json(prefer_path, kz, &gamma_in_file);
    if (!ok && prefer_path != "../data/kz.json") {
      ok = load_kz_json("../data/kz.json", kz, &gamma_in_file);
      if (ok) prefer_path = "../data/kz.json";
    }
    if (!ok) {
      throw std::runtime_error("requester: cannot load controller for group "
                               + std::to_string(gid) + " (name='" + group_names[gid]
                               + "'), tried: " + prefer_path);
    }

    filts.emplace_back(DF2T{kz});
    kz_used_path[gid] = prefer_path;
  }

  // 为每组创建日志目录
  for (size_t gid = 0; gid < dst_groups.size(); ++gid) {
    ensure_log_dir(group_log_dir(gid, group_names[gid]));
  }

  // 每组误差历史（画独立 SVG）
  std::vector<std::vector<double>> e_hist_groups(dst_groups.size());

  // ==== 新增：稳定性参数与每组状态 ====
  static constexpr int64_t ERR_THRESH_NS = 1'000'000; // 1 ms（大误差阈值）
  int window_W = 100;                                  // 最近 W 轮窗口
  if (const char* envW = std::getenv("TSYNC_WIN_W")) {
    try {
      int w = std::stoi(envW);
      if (w >= 8 && w <= 100000) window_W = w;
    } catch (...) {}
  }
  const int stable_need_good_streak = 50;              // 连续 50 正常轮 → 稳定
  struct GroupStab {
    bool  is_stable = false;
    int   good_streak = 0;
    std::deque<int> recent_bad; // 1=异常, 0=正常
    int   bad_count = 0;
  };
  std::vector<GroupStab> gstab(dst_groups.size());

  // 主循环
  using clk = std::chrono::steady_clock;
  const auto period = std::chrono::nanoseconds((long long)tau_ns);
  auto next_tp = clk::now() + period;
  uint32_t k = 0;

  size_t total_peers = 0;
  for (auto& v : dst_groups) total_peers += v.size();

  std::cout << "[requester] start, tau=" << (double)tau_ns/1e6
            << " ms, groups=" << dst_groups.size()
            << " (total_peers=" << total_peers << ")"
            << ", k_limit=" << k_limit
            << ", W=" << window_W
            << ", stable_need=" << stable_need_good_streak
            << "\n";
  for (size_t gid=0; gid<dst_groups.size(); ++gid) {
    std::cout << "  - group#" << gid
              << (group_names[gid].empty()? "" : (" [" + group_names[gid] + "]"))
              << " -> controller: " << kz_used_path[gid] << "\n";
  }

  SyncNtpEstimator estimator(/*min_win=*/32);

  while (g_running.load(std::memory_order_relaxed)) {
    const uint32_t round_k = ++k;
    const int32_t  r_i_ppm = 0;
    int alertCount = 0;

    // ==== 按组独立：扇出→扇入→聚合→控制 ====
    for (size_t gid = 0; gid < dst_groups.size(); ++gid) {
      const auto& dsts = dst_groups[gid];
      if (dsts.empty()) continue;

      // 扇出：给该组全部邻居发 REQ
      for (const auto& dst : dsts) {
        SyncNtpMsg req = SyncNtpMsg::make_req(round_k, tau_ns, r_i_ppm, lc);
        SyncNtpMsg req_net = req; req_net.hton();
        if (sendto(fd, (const char*)&req_net, sizeof(req_net), 0,
                   (const sockaddr*)&dst, (socklen_t)sizeof(dst)) < 0) {
          perror("[requester] sendto");
        }
      }

      // 扇入：只收该组的 RESP
      const auto deadline = clk::now() + std::chrono::milliseconds(recv_window_ms);
      std::vector<char> got(dsts.size(), 0);
      std::vector<double> e_samples_ns;
      e_samples_ns.reserve(dsts.size());

      std::vector<PeerDetail> peer_details;
      peer_details.reserve(dsts.size());

      auto count_got = [&](){
        return (int)std::count(got.begin(), got.end(), 1);
      };

      auto find_neighbor_index_in_group = [&](const sockaddr_in& src)->int {
        for (size_t i=0;i<dsts.size();++i) {
          if (src.sin_addr.s_addr == dsts[i].sin_addr.s_addr &&
              src.sin_port        == dsts[i].sin_port) return (int)i;
        }
        return -1;
      };

      while (g_running.load(std::memory_order_relaxed) && clk::now() < deadline) {
        SyncNtpMsg resp_net{}; sockaddr_in src{}; socklen_t sl = sizeof(src);
        ssize_t n = recvfrom(fd, (char*)&resp_net, sizeof(resp_net), 0, (sockaddr*)&src, &sl);
        const uint64_t t4_ns = (uint64_t)lc.now_ns();
        if (n < (ssize_t)sizeof(SyncNtpMsg)) continue;

        // 只处理当前轮次的 RESP
        SyncNtpMsg peek = resp_net; peek.ntoh();
        if (peek.mode != 2 || peek.k != round_k) continue;

        // 必须来自当前组内的邻居
        int idx = find_neighbor_index_in_group(src);
        if (idx < 0 || got[idx]) continue;

        auto est = estimator.estimate_from_resp(resp_net, t4_ns, /*t1_fallback*/0);
        if (!est.ok) continue;

        // 源地址
        char ip_str[INET_ADDRSTRLEN];
        inet_ntop(AF_INET, &src.sin_addr, ip_str, INET_ADDRSTRLEN);
        uint16_t port = ntohs(src.sin_port);

        // 记录详情
        PeerDetail detail;
        detail.ip = ip_str;
        detail.port = port;
        detail.t1_ns = est.t1_ns;
        detail.t2_ns = est.t2_ns;
        detail.t3_ns = est.t3_ns;
        detail.t4_ns = est.t4_ns;
        detail.offset_ns = est.offset_ns;
        detail.delay_ns = est.delay_ns;
        detail.error_ns  = static_cast<double>(est.offset_ns);
        peer_details.push_back(detail);

        const double e_ns = -(double)est.offset_ns;
        e_samples_ns.push_back(e_ns);
        got[idx] = 1;

        if (count_got() == (int)dsts.size()) break; // 收齐提前结束
      }

      // === 只要收到了样本，就聚合并控制 ===
      if (!e_samples_ns.empty()) {
        // 组内平均误差
        double mean_e = 0.0;
        double mean_abs_e = 0.0;
        auto [min_it, max_it] = std::minmax_element(e_samples_ns.begin(), e_samples_ns.end());
        double min_e = *min_it;
        double max_e = *max_it;

        for (double v : e_samples_ns) {
          mean_e += v;
          mean_abs_e += std::fabs(v);
        }
        mean_e     /= (double)e_samples_ns.size();
        mean_abs_e /= (double)e_samples_ns.size();

        // ==== 新：本轮异常/正常判定 ====
        bool neighbor_has_abnormal = false;
        for (const auto& detail : peer_details) {
          if (std::llabs((long long)std::llround(detail.error_ns)) > ERR_THRESH_NS) {
            neighbor_has_abnormal = true;
            break;
          }
        }
        bool mean_e_abnormal = (std::llabs((long long)std::llround(mean_e)) > ERR_THRESH_NS);
        bool is_good_round = (!neighbor_has_abnormal && !mean_e_abnormal);
        bool is_abnormal_round = !is_good_round;

        // ==== 更新稳定性状态机（每组独立）====
        auto& ST = gstab[gid];
        // 维护最近 W 轮窗口
        if ((int)ST.recent_bad.size() == window_W) {
          ST.bad_count -= ST.recent_bad.front();
          ST.recent_bad.pop_front();
        }
        ST.recent_bad.push_back(is_abnormal_round ? 1 : 0);
        ST.bad_count += (is_abnormal_round ? 1 : 0);

        // 维护连续好轮计数
        if (is_good_round) ST.good_streak++;
        else ST.good_streak = 0;

        // 状态切换
        bool new_stable = ST.is_stable;
        const int unstable_threshold = (window_W + 3) / 4; // ceil(W/4)
        if (!ST.is_stable && ST.good_streak >= stable_need_good_streak) {
          new_stable = true;
        } else if (ST.bad_count >= unstable_threshold) {
          new_stable = false;
        }
        if (new_stable != ST.is_stable) {
          ST.is_stable = new_stable;
          std::cout << "[stability] k=" << round_k
                    << " group#" << gid
                    << (group_names[gid].empty()? "" : (" [" + group_names[gid] + "]"))
                    << " -> " << (ST.is_stable ? "STABLE" : "UNSTABLE")
                    << " (good_streak=" << ST.good_streak
                    << ", bad_in_W=" << ST.bad_count << "/" << window_W << ")\n";
        }

        // === 更新前快照 ===
        const int64_t t_logic_before_ns = (int64_t)lc.now_ns();
        const int64_t t_raw_ns          = (int64_t)lc_raw.now_ns();
        const int64_t offset_before     = (int64_t)lc.offset_ns();

        // 更新该组折线图（用有符号平均误差）
        const std::string gdir = group_log_dir(gid, group_names[gid]);
        ensure_log_dir(gdir);
        {
          std::ostringstream title;
          if (group_names[gid].empty()) {
            title << "e_avg per round (ns) - group#" << gid;
          } else {
            title << "e_avg per round (ns) - " << group_names[gid];
          }
          e_hist_groups[gid].push_back(mean_e);
       //   write_eavg_svg(e_hist_groups[gid], gdir + "eavg.svg", title.str());
        }

        // 控制器输出（ns）
        double  mu   = filts[gid].step(mean_e) * k_limit;
        int64_t mu_i = (int64_t)std::llround(mu);

        // === 按系统稳定性选择对齐策略 ===
        bool ok_update = false;
        const char* mode_str = nullptr;
        if (ST.is_stable) {
          if (!is_abnormal_round) {
            ok_update = lc.apply_mu_good_map(/*mu_ns=*/mu_i,
                                             /*tau_ns=*/(int64_t)tau_ns,
                                             /*s_abs_max_ppm=*/0.0,
                                             /*k_capacity=*/32);
            mode_str = "STABLE:GOOD_MAP";
          } else {
            ok_update = lc.apply_mu_bad_fit(/*s_abs_max_ppm=*/0.0, /*k_capacity=*/32);
            mode_str = "STABLE:BAD_FIT";
          }
        } else {
          // 非稳定：统一快速追齐
          ok_update = lc.apply_mu_good_map(/*mu_ns=*/mu_i,
                                           /*tau_ns=*/(int64_t)tau_ns,
                                           /*s_abs_max_ppm=*/0.0,
                                           /*k_capacity=*/32);
          mode_str = "UNSTABLE:GOOD_MAP";
        }
        (void)ok_update; // 需要兜底可启用：if (!ok_update) lc.add_offset_step(mu_i);

        // === 更新后快照 ===
        const double  slope_ppm        = lc.slope_ppm();
        const int64_t t_logic_after_ns = (int64_t)lc.now_ns();
        const int64_t offset_after     = (int64_t)lc.offset_ns();

        // === 写入 CSV（带 before/after 与斜率）===
        append_metrics_csv_ex(gdir + metrics_path,
                              round_k,
                              /*mean_abs_e*/ mean_abs_e,
                              /*mean_e*/     mean_e,
                              /*max_e*/      max_e,
                              /*min_e*/      min_e,
                              /*t_logic_before_ns*/ t_logic_before_ns,
                              /*t_raw_ns*/          t_raw_ns,
                              /*offset_before_ns*/  offset_before,
                              /*slope_ppm*/         slope_ppm,
                              /*t_logic_after_ns*/  t_logic_after_ns,
                              /*offset_after_ns*/   offset_after);

        // 周期打印
        if (round_k % 50 == 0) {
          std::cout << "[requester] k=" << round_k
            << " group#" << gid
            << (group_names[gid].empty()? "" : (" [" + group_names[gid] + "]"))
            << " ctrl=" << kz_used_path[gid]
            << " got=" << peer_details.size() << "/" << dsts.size()
            << " mean_abs_e=" << mean_abs_e << " ns"
            << " mean_e=" << mean_e << " ns"
            << " max_e=" << max_e << " ns"
            << " min_e=" << min_e << " ns"
            << " mu_raw=" << mu << " ns"
            << " mu_i=" << mu_i << " ns"
            << " mode=" << mode_str
            << " stable=" << (ST.is_stable ? 1 : 0)
            << " good_streak=" << ST.good_streak
            << " bad_in_W=" << ST.bad_count << "/" << window_W
            << " slope_ppm=" << slope_ppm
            << " logic_before=" << t_logic_before_ns << " ns"
            << " logic_after="  << t_logic_after_ns  << " ns"
            << " offset_before=" << offset_before << " ns"
            << " offset_after="  << offset_after  << " ns"
            << " alertCount=" << alertCount << "/"<<round_k
            << "\n";

          for (const auto& detail : peer_details) {
            std::cout << "    Peer: " << detail.ip << ":" << detail.port
                      << " | T1: " << detail.t1_ns
                      << " | T2: " << detail.t2_ns
                      << " | T3: " << detail.t3_ns
                      << " | T4: " << detail.t4_ns
                      << " | Offset: " << detail.offset_ns << " ns"
                      << " | Delay: " << detail.delay_ns << " ns"
                      << " | Error: " << detail.error_ns << " ns\n";
          }
        }

        // 告警：k>1000 且 |e_avg|>1ms（保留原有告警逻辑）
        static constexpr int64_t ONE_MS_NS = 1'000'000;
        if (round_k > 1000 && std::llabs((long long)std::llround(mean_e)) > ONE_MS_NS) {
          alertCount++;
          std::cout << "[ALERT] k=" << round_k
                    << " group#" << gid
                    << (group_names[gid].empty()? "" : (" [" + group_names[gid] + "]"))
                    << " e_avg=" << mean_e << " ns (>1 ms)"
                    << " mean_abs_e=" << mean_abs_e << " ns"
                    << " max_e=" << max_e << " ns"
                    << " min_e=" << min_e << " ns"
                    << " got=" << peer_details.size() << "/" << dsts.size()
                    << " mu_raw=" << mu << " ns"
                    << " mu_i=" << mu_i << " ns"
                    << " mode=" << mode_str
                    << " stable=" << (ST.is_stable ? 1 : 0)
                    << " good_streak=" << ST.good_streak
                    << " bad_in_W=" << ST.bad_count << "/" << window_W
                    << " slope_ppm=" << slope_ppm
                    << " logic_before=" << t_logic_before_ns << " ns"
                    << " logic_after="  << t_logic_after_ns  << " ns"
                    << " offset_before=" << offset_before << " ns"
                    << " offset_after="  << offset_after  << " ns"
                    << "\n";

          for (const auto& d : peer_details) {
            std::cout << "    Peer: " << d.ip << ":" << d.port
                      << " | T1: " << d.t1_ns
                      << " | T2: " << d.t2_ns
                      << " | T3: " << d.t3_ns
                      << " | T4: " << d.t4_ns
                      << " | Offset: " << d.offset_ns << " ns"
                      << " | Delay: " << d.delay_ns << " ns"
                      << " | Error: " << d.error_ns << " ns\n";
          }
        }

      } else {
        std::cout << "[requester] k=" << round_k
                  << " group#" << gid
                  << (group_names[gid].empty()? "" : (" [" + group_names[gid] + "]"))
                  << " ctrl=" << kz_used_path[gid]
                  << " no response in window\n";
      }

    } // end for each group

    // === 等到下一个周期 ===
    std::this_thread::sleep_until(next_tp);
    do { next_tp += period; } while (next_tp <= clk::now());
  }

#if defined(_WIN32)
  closesocket(fd);
#else
  close(fd);
#endif
}

// ---- 对外 API：同时启用常驻 Responder + 周期性 Requester ----
void run_controller(std::vector<Neighbor>& neighbors,
                    uint32_t tau_ns,
                    const char* bind_ip,
                    int recv_window_ms,
                    uint16_t listen_port,
                    double k_limit,
                    LogicalClock& lc, const std::string &metricsPath)   // ★ 外部传入时钟
{
  metrics_path = metricsPath;
  // 默认值兜底 & 环境变量覆盖
  if (!(k_limit > 0.0) || !std::isfinite(k_limit)) {
    k_limit = 0.02;
  }
  if (const char* env = std::getenv("TSYNC_KLIMIT")) {
    try {
      double kv = std::stod(env);
      if (kv > 0.0 && std::isfinite(kv)) k_limit = kv;
    } catch (...) { /* ignore parse error */ }
  }

  auto cal = TscSource::init(0.3);   // 0.3s 校准窗口
  if (!cal.ok) {
    std::fprintf(stderr, "[FATAL] Invariant TSC 不可用或校准失败；请启用回退或退出。\n");
  }
  g_running.store(true, std::memory_order_relaxed);

  // 启动被动回包线程（listen_port>0 才启用）
  std::thread th_responder;
  if (listen_port != 0) {
    th_responder = std::thread([&](){
      responder_loop(bind_ip, listen_port, lc);
    });
    std::this_thread::sleep_for(std::chrono::milliseconds(10)); // 让 responder 先 bind
  }

  // 在当前线程运行 Requester（阻塞）
  try {
    requester_loop(neighbors, tau_ns, bind_ip, recv_window_ms, k_limit, lc);
  } catch (...) {
    g_running.store(false, std::memory_order_relaxed);
    if (th_responder.joinable()) th_responder.join();
    throw;
  }

  // 正常退出
  g_running.store(false, std::memory_order_relaxed);
  if (th_responder.joinable()) th_responder.join();
}
//
// // ---- 兼容重载：仅主动对齐（不启用 Responder）----
// void run_controller(std::vector<Neighbor>& neighbors,
//                     uint32_t tau_ns,
//                     const char* bind_ip,
//                     int recv_window_ms,
//                     double k_limit,
//                     LogicalClock& lc)   // ★ 外部传入时钟
// {
//   run_controller(neighbors, tau_ns, bind_ip, recv_window_ms,
//                  /*listen_port=*/0, k_limit, lc,"m");
// }
//
 } // namespace tsync
